//////////////////////////////////////////////////////////////////////////////////
//                                                                              //
//  This file is part of the buola project (https://code.google.com/p/buola/).  //
//                                                                              //
//  Copyright(c) 2007-2012 Xavi Gratal                                          //
//  gratal AT gmail DOT com                                                     //
//                                                                              //
//  Buola is free software: you can redistribute it and/or modify               //
//  it under the terms of the GNU General Public License as published by        //
//  the Free Software Foundation, either version 3 of the License, or           //
//  (at your option) any later version.                                         //
//                                                                              //
//  Buola is distributed in the hope that it will be useful,                    //
//  but WITHOUT ANY WARRANTY; without even the implied warranty of              //
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the               //
//  GNU General Public License for more details.                                //
//                                                                              //
//  You should have received a copy of the GNU General Public License           //
//  along with buola.  If not, see <http://www.gnu.org/licenses/>.              //
//                                                                              //
//////////////////////////////////////////////////////////////////////////////////

#include <buola/image/vertex/cbspline.h>

namespace buola { namespace img {

void CBSpline::Prepare()
{
    mLastI=-1;

    if(mPoints.size()<=2) return;

    std::vector<double> lTemp(3*mPoints.size(),0.0);
    double *lR=&lTemp[mPoints.size()];
    double *lS=&lTemp[mPoints.size()*2];
    
    int lN1=mPoints.size()-1;

    double d=mPoints[1].mX-mPoints[0].mX;
    double e=(mPoints[1].mY-mPoints[0].mY)/d;

    for(int k=1;k<lN1;k++)
    {
        double h=d,f=e;
        d=mPoints[k+1].mX-mPoints[k].mX;
        e=(mPoints[k+1].mY-mPoints[k].mY)/d;
        lTemp[k]=d/(d+h);
        lR[k]=1.0-lTemp[k];
        lS[k]=6.0*(e-f)/(h+d);
    }

    for(int k=1;k<lN1;k++)
    {
        double p=1.0/(lR[k]*lTemp[k-1]+2.0);
        lTemp[k]*=-p;
        lS[k]=(lS[k]-lR[k]*lS[k-1])*p;
    }

    mPoints[lN1].mA=0.0;
    lTemp[lN1-1]=lS[lN1-1];
    mPoints[lN1-1].mA=lTemp[lN1-1];

    for(int k=lN1-2;k>=0;k--)
    {
        lTemp[k]=lTemp[k]*lTemp[k+1]+lS[k];
        mPoints[k].mA=lTemp[k];
    }
}

int CBSpline::BSearch(double pX,int i,int j) const
{
    while(j-i>1)
    {
        int k=(i+j)>>1;
        if(pX<mPoints[k].mX)
            j=k;
        else
            i=k;
    }

    return i;
}

double CBSpline::Interpolation(double pX,int i) const
{
    double d=mPoints[i].mX-mPoints[i+1].mX;
    double h=pX-mPoints[i+1].mX;
    double r=mPoints[i].mX-pX;
    double p=sq(d)/6.0;
    return (mPoints[i+1].mA*r*r*r+mPoints[i].mA*h*h*h)/6.0/d+
           ((mPoints[i+1].mY-mPoints[i+1].mA*p)*r+(mPoints[i].mY-mPoints[i].mA*p)*h)/d;
}

double CBSpline::ExtrapolationLeft(double pX) const
{
    double d=mPoints[1].mX-mPoints[0].mX;
    return (-d*mPoints[1].mA/6.0+(mPoints[1].mY-mPoints[0].mY)/d)*(pX-mPoints[0].mX)+mPoints[0].mY;
}

double CBSpline::ExtrapolationRight(double pX) const
{
    const auto &l1=mPoints[mPoints.size()-1];
    const auto &l2=mPoints[mPoints.size()-2];
    double d=l1.mX-l2.mX;
    return (d*l2.mA/6.0+(l1.mY-l2.mY)/d)*(pX-l1.mX)+l1.mY;
}

double CBSpline::Get(double pX) const
{
    if(pX<mPoints.front().mX)
        return ExtrapolationLeft(pX);

    if(pX>=mPoints.back().mX)
        return ExtrapolationRight(pX);

    return Interpolation(pX,BSearch(pX,0,mPoints.size()-1));
}

double CBSpline::GetNext(double pX) const
{
    if(pX<mPoints.front().mX)
        return ExtrapolationLeft(pX);

    if(pX>=mPoints.back().mX)
        return ExtrapolationRight(pX);

    int i=mLastI;

    if(i>=0) //we stored previous position
    {
        if(pX>mPoints[i+1].mX) //to the right
        {
            if(pX<=mPoints[i+2].mX) //immediately to the right (most likely)
                i++;
            else //full search to the right
                i=BSearch(pX,i+2,mPoints.size()-1);
        }
        else if(pX<mPoints[i].mX) //to the right
        {
            if(pX>=mPoints[i-1].mX) //immediately to the left (most likely)
                i--;
            else //full search to the right
                i=BSearch(pX,0,i-1);
        }
    }
    else
    {
        i=BSearch(pX,0,mPoints.size()-1);
    }

    mLastI=i;

    return Interpolation(pX,i);
}

/*namespace img*/ } /*namespace buola*/ }
